/*
 * SPDX-FileCopyrightText: 2021-2024 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Unlicense OR CC0-1.0
 */
#include <sys/time.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_bt.h"
#include "driver/uart.h"
#include "string.h"
#include "esp_sleep.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include <math.h>
#include "driver/rmt_tx.h"

#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_bt_defs.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"
#include "esp_gatt_common_api.h"

#include "iStetho3_ble.h"
#include "iStetho3_util.h"
#include "u8g2_esp32.h"

#define BLE_TAG "iStetho2_BLE"

#define SPP_PROFILE_NUM 1
#define SPP_PROFILE_APP_IDX 0
#define ESP_SPP_APP_ID 0x56
#define SPP_SVC_INST_ID 0

// #define SAMPLE_DEVICE_NAME "iStetho3-SN0001" // The Device Name Characteristics in GAP
char ADVTISE_DEVICE_NAME[17] = "iStetho2-SN0000";
char  devSN[5] = "0000";
/// SPP Service
static const uint16_t spp_service_uuid = 0xABF0;
/// Characteristic UUID
#define ESP_GATT_UUID_SPP_DATA_RECEIVE 0xABF1
#define ESP_GATT_UUID_SPP_DATA_NOTIFY 0xABF2
#define ESP_GATT_UUID_SPP_COMMAND_RECEIVE 0xABF3
#define ESP_GATT_UUID_SPP_COMMAND_NOTIFY 0xABF4

#ifdef SUPPORT_HEARTBEAT
#define ESP_GATT_UUID_SPP_HEARTBEAT 0xABF5
#endif

uint8_t spp_adv_data[25] = {
    /* Flags */
    0x02, ESP_BLE_AD_TYPE_FLAG, 0x06,
    /* Complete List of 16-bit Service Class UUIDs */
    0x03, ESP_BLE_AD_TYPE_16SRV_CMPL, 0xF0, 0xAB,
    /* Complete Local Name in advertising */
    // 0x09, ESP_BLE_AD_TYPE_NAME_CMPL, 'i', 'S', 't', 'e', 't', 'h', 'o', '3'};
    0x10, ESP_BLE_AD_TYPE_NAME_CMPL, 'i', 'S', 't', 'e', 't', 'h', 'o', '2', '-', 'S', 'N', '0', '0', '0', '0', 0};

static uint16_t spp_mtu_size = 263;
static uint16_t spp_conn_id = 0xffff;
static esp_gatt_if_t spp_gatts_if = 0xff;
QueueHandle_t spp_uart_queue = NULL;
static QueueHandle_t ble_cmd_queue = NULL;

#ifdef SUPPORT_HEARTBEAT
static QueueHandle_t cmd_heartbeat_queue = NULL;
static uint8_t heartbeat_s[8] = {'i', 'S', 't', 'e', 't', 'h', 'o', '2'};
static bool enable_heart_ntf = false;
static uint8_t heartbeat_count_num = 0;
#endif

static bool enable_data_ntf = false;
static bool enable_status_ntf = false;
static bool is_connected = false;
static esp_bd_addr_t spp_remote_bda = {
    0x0,
};

static uint16_t spp_handle_table[SPP_IDX_NB];

static esp_ble_adv_params_t spp_adv_params = {
    .adv_int_min = 0x20,
    .adv_int_max = 0x40,
    .adv_type = ADV_TYPE_IND,
    .own_addr_type = BLE_ADDR_TYPE_PUBLIC,
    .channel_map = ADV_CHNL_ALL,
    .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};

struct gatts_profile_inst
{
    esp_gatts_cb_t gatts_cb;
    uint16_t gatts_if;
    uint16_t app_id;
    uint16_t conn_id;
    uint16_t service_handle;
    esp_gatt_srvc_id_t service_id;
    uint16_t char_handle;
    esp_bt_uuid_t char_uuid;
    esp_gatt_perm_t perm;
    esp_gatt_char_prop_t property;
    uint16_t descr_handle;
    esp_bt_uuid_t descr_uuid;
};

typedef struct spp_receive_data_node
{
    int32_t len;
    uint8_t *node_buff;
    struct spp_receive_data_node *next_node;
} spp_receive_data_node_t;

static spp_receive_data_node_t *temp_spp_recv_data_node_p1 = NULL;
static spp_receive_data_node_t *temp_spp_recv_data_node_p2 = NULL;

typedef struct spp_receive_data_buff
{
    int32_t node_num;
    int32_t buff_size;
    spp_receive_data_node_t *first_node;
} spp_receive_data_buff_t;

static spp_receive_data_buff_t SppRecvDataBuff = {
    .node_num = 0,
    .buff_size = 0,
    .first_node = NULL};

static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param);

/* One gatt-based profile one app_id and one gatts_if, this array will store the gatts_if returned by ESP_GATTS_REG_EVT */
static struct gatts_profile_inst spp_profile_tab[SPP_PROFILE_NUM] = {
    [SPP_PROFILE_APP_IDX] = {
        .gatts_cb = gatts_profile_event_handler,
        .gatts_if = ESP_GATT_IF_NONE, /* Not get the gatt_if, so initial is ESP_GATT_IF_NONE */
    },
};

/*
 *  SPP PROFILE ATTRIBUTES
 ****************************************************************************************
 */

#define CHAR_DECLARATION_SIZE (sizeof(uint8_t))
static const uint16_t primary_service_uuid = ESP_GATT_UUID_PRI_SERVICE;
static const uint16_t character_declaration_uuid = ESP_GATT_UUID_CHAR_DECLARE;
static const uint16_t character_client_config_uuid = ESP_GATT_UUID_CHAR_CLIENT_CONFIG;

static const uint8_t char_prop_read_notify = ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_NOTIFY;
static const uint8_t char_prop_read_write = ESP_GATT_CHAR_PROP_BIT_WRITE_NR | ESP_GATT_CHAR_PROP_BIT_READ;

#ifdef SUPPORT_HEARTBEAT
static const uint8_t char_prop_read_write_notify = ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_WRITE_NR | ESP_GATT_CHAR_PROP_BIT_NOTIFY;
#endif

/// SPP Service - data receive characteristic, read&write without response
static const uint16_t spp_data_receive_uuid = ESP_GATT_UUID_SPP_DATA_RECEIVE;
static const uint8_t spp_data_receive_val[20] = {0x00};

/// SPP Service - data notify characteristic, notify&read
static const uint16_t spp_data_notify_uuid = ESP_GATT_UUID_SPP_DATA_NOTIFY;
static const uint8_t spp_data_notify_val[20] = {0x00};
static const uint8_t spp_data_notify_ccc[2] = {0x00, 0x00};

/// SPP Service - command characteristic, read&write without response
static const uint16_t spp_command_uuid = ESP_GATT_UUID_SPP_COMMAND_RECEIVE;
static const uint8_t spp_command_val[10] = {0x00};

/// SPP Service - status characteristic, notify&read
static const uint16_t spp_status_uuid = ESP_GATT_UUID_SPP_COMMAND_NOTIFY;
static const uint8_t spp_status_val[10] = {0x00};
static const uint8_t spp_status_ccc[2] = {0x00, 0x00};

#ifdef SUPPORT_HEARTBEAT
/// SPP Server - Heart beat characteristic, notify&write&read
static const uint16_t spp_heart_beat_uuid = ESP_GATT_UUID_SPP_HEARTBEAT;
static const uint8_t spp_heart_beat_val[2] = {0x00, 0x00};
static const uint8_t spp_heart_beat_ccc[2] = {0x00, 0x00};
#endif

/// Full HRS Database Description - Used to add attributes into the database
static const esp_gatts_attr_db_t spp_gatt_db[SPP_IDX_NB] =
    {
        // SPP -  Service Declaration
        [SPP_IDX_SVC] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&primary_service_uuid, ESP_GATT_PERM_READ, sizeof(spp_service_uuid), sizeof(spp_service_uuid), (uint8_t *)&spp_service_uuid}},

        // SPP -  data receive characteristic Declaration
        [SPP_IDX_SPP_DATA_RECV_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write}},

        // SPP -  data receive characteristic Value
        [SPP_IDX_SPP_DATA_RECV_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_data_receive_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, SPP_DATA_MAX_LEN, sizeof(spp_data_receive_val), (uint8_t *)spp_data_receive_val}},

        // SPP -  data notify characteristic Declaration
        [SPP_IDX_SPP_DATA_NOTIFY_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_notify}},

        // SPP -  data notify characteristic Value
        [SPP_IDX_SPP_DATA_NTY_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_data_notify_uuid, ESP_GATT_PERM_READ, SPP_DATA_MAX_LEN, sizeof(spp_data_notify_val), (uint8_t *)spp_data_notify_val}},

        // SPP -  data notify characteristic - Client Characteristic Configuration Descriptor
        [SPP_IDX_SPP_DATA_NTF_CFG] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(spp_data_notify_ccc), (uint8_t *)spp_data_notify_ccc}},

        // SPP -  command characteristic Declaration
        [SPP_IDX_SPP_COMMAND_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write}},

        // SPP -  command characteristic Value
        [SPP_IDX_SPP_COMMAND_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_command_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, SPP_CMD_MAX_LEN, sizeof(spp_command_val), (uint8_t *)spp_command_val}},

        // SPP -  status characteristic Declaration
        [SPP_IDX_SPP_STATUS_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_notify}},

        // SPP -  status characteristic Value
        [SPP_IDX_SPP_STATUS_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_status_uuid, ESP_GATT_PERM_READ, SPP_STATUS_MAX_LEN, sizeof(spp_status_val), (uint8_t *)spp_status_val}},

        // SPP -  status characteristic - Client Characteristic Configuration Descriptor
        [SPP_IDX_SPP_STATUS_CFG] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(spp_status_ccc), (uint8_t *)spp_status_ccc}},

#ifdef SUPPORT_HEARTBEAT
        // SPP -  Heart beat characteristic Declaration
        [SPP_IDX_SPP_HEARTBEAT_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write_notify}},

        // SPP -  Heart beat characteristic Value
        [SPP_IDX_SPP_HEARTBEAT_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_heart_beat_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(spp_heart_beat_val), sizeof(spp_heart_beat_val), (uint8_t *)spp_heart_beat_val}},

        // SPP -  Heart beat characteristic - Client Characteristic Configuration Descriptor
        [SPP_IDX_SPP_HEARTBEAT_CFG] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(spp_data_notify_ccc), (uint8_t *)spp_heart_beat_ccc}},
#endif
};

static uint8_t find_char_and_desr_index(uint16_t handle)
{
    uint8_t error = 0xff;

    for (int i = 0; i < SPP_IDX_NB; i++)
    {
        if (handle == spp_handle_table[i])
        {
            return i;
        }
    }

    return error;
}

static bool store_wr_buffer(esp_ble_gatts_cb_param_t *p_data)
{
    temp_spp_recv_data_node_p1 = (spp_receive_data_node_t *)malloc(sizeof(spp_receive_data_node_t));

    if (temp_spp_recv_data_node_p1 == NULL)
    {
        ESP_LOGI(BLE_TAG, "malloc error %s %d", __func__, __LINE__);
        return false;
    }
    if (temp_spp_recv_data_node_p2 != NULL)
    {
        temp_spp_recv_data_node_p2->next_node = temp_spp_recv_data_node_p1;
    }
    temp_spp_recv_data_node_p1->len = p_data->write.len;
    SppRecvDataBuff.buff_size += p_data->write.len;
    temp_spp_recv_data_node_p1->next_node = NULL;
    temp_spp_recv_data_node_p1->node_buff = (uint8_t *)malloc(p_data->write.len);
    temp_spp_recv_data_node_p2 = temp_spp_recv_data_node_p1;
    if (temp_spp_recv_data_node_p1->node_buff == NULL)
    {
        ESP_LOGI(BLE_TAG, "malloc error %s %d\n", __func__, __LINE__);
        temp_spp_recv_data_node_p1->len = 0;
    }
    else
    {
        memcpy(temp_spp_recv_data_node_p1->node_buff, p_data->write.value, p_data->write.len);
    }

    if (SppRecvDataBuff.node_num == 0)
    {
        SppRecvDataBuff.first_node = temp_spp_recv_data_node_p1;
        SppRecvDataBuff.node_num++;
    }
    else
    {
        SppRecvDataBuff.node_num++;
    }

    return true;
}

static void free_write_buffer(void)
{
    temp_spp_recv_data_node_p1 = SppRecvDataBuff.first_node;

    while (temp_spp_recv_data_node_p1 != NULL)
    {
        temp_spp_recv_data_node_p2 = temp_spp_recv_data_node_p1->next_node;
        if (temp_spp_recv_data_node_p1->node_buff)
        {
            free(temp_spp_recv_data_node_p1->node_buff);
        }
        free(temp_spp_recv_data_node_p1);
        temp_spp_recv_data_node_p1 = temp_spp_recv_data_node_p2;
    }

    SppRecvDataBuff.node_num = 0;
    SppRecvDataBuff.buff_size = 0;
    SppRecvDataBuff.first_node = NULL;
}

static void print_write_buffer(void)
{
    temp_spp_recv_data_node_p1 = SppRecvDataBuff.first_node;

    while (temp_spp_recv_data_node_p1 != NULL)
    {
        uart_write_bytes(UART_NUM_0, (char *)(temp_spp_recv_data_node_p1->node_buff), temp_spp_recv_data_node_p1->len);
        // usb_serial_jtag_write_bytes((const void *)(temp_spp_recv_data_node_p1->node_buff), temp_spp_recv_data_node_p1->len, portMAX_DELAY);
        temp_spp_recv_data_node_p1 = temp_spp_recv_data_node_p1->next_node;
    }
}

/* ************************************************************************** */
//  record_send_data
//
//      send the record date from the record filter to usb or ble
//        
/* ************************************************************************** */
uint32_t  g_uRCDNum = 0;
bool      g_bOutputUart = false;
uint8_t   ntf_value_p[518];

uint32_t record_send_data(char *buffer, int wanted_size)
{
    uint8_t total_num = 0;
    uint8_t current_num = 0;

    uint8_t *temp = (uint8_t *)buffer;
    int readSize = wanted_size;
    g_uRCDNum++;
    
    if ((readSize>0) &&(!is_connected) && (!enable_data_ntf)){
        // ESP_LOGI(BLE_TAG,"record_send_data by uart size %d.\r\n",wanted_size);
        if ( g_bOutputUart)
        {
            uart_write_bytes(UART_NUM_0, buffer, wanted_size); 
            // usb_serial_jtag_write_bytes((const char *) buffer, readSize, 20 / portTICK_PERIOD_MS);
        }
        return  wanted_size;  
    }
    while((readSize > 0)&&(is_connected))
    {
        #ifdef SUPPORT_HEARTBEAT
        if(!enable_heart_ntf){
            ESP_LOGE(BLE_TAG, "%s do not enable heartbeat Notify\n", __func__);
            break;
        }
        #endif
        if(!enable_data_ntf){
            // unsigned portBASE_TYPE uxHighWaterMark=uxTaskGetStackHighWaterMark( NULL );
            // ESP_LOGW(BLE_TAG,"Main task left heap size %d.\r\n",uxHighWaterMark);
            // ESP_LOGE(BLE_TAG, "%s do not enable data Notify\n", __func__);
            break;
        }
        
        if(readSize <= (spp_mtu_size - 3)){
            esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_DATA_NTY_VAL],readSize, temp, false);
        }else if(readSize > (spp_mtu_size - 3)){

            if((readSize%(spp_mtu_size - 7)) == 0){
                total_num = readSize/(spp_mtu_size - 7);
            }else{
                total_num = readSize/(spp_mtu_size - 7) + 1;
            }

            current_num = 1;
            while(current_num <= total_num){
                if(current_num < total_num){
                    ntf_value_p[0] = '#';
                    ntf_value_p[1] = '#';
                    //ntf_value_p[2] = g_uRCDNum / 256;
                    ntf_value_p[2] = g_uRCDNum % 256;
                    //ntf_value_p[2] = total_num;
                    ntf_value_p[3] = current_num;
                    memcpy(ntf_value_p + 4,temp + (current_num - 1)*(spp_mtu_size-7),(spp_mtu_size-7));
                    esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_DATA_NTY_VAL],(spp_mtu_size-3), ntf_value_p, false);
                }else if(current_num == total_num){
                    ntf_value_p[0] = '#';
                    ntf_value_p[1] = '#';
                    //ntf_value_p[2] = g_uRCDNum / 256;
                    ntf_value_p[2] = g_uRCDNum % 256;
                    //ntf_value_p[2] = total_num;
                    ntf_value_p[3] = current_num;
                    memcpy(ntf_value_p + 4,temp + (current_num - 1)*(spp_mtu_size-7),(readSize - (current_num - 1)*(spp_mtu_size - 7)));
                    esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_DATA_NTY_VAL],(readSize - (current_num - 1)*(spp_mtu_size - 7) + 4), ntf_value_p, false);
                }
                vTaskDelay(10 / portTICK_PERIOD_MS);
                current_num++;
            }
        }
        // ESP_LOGI(BLE_TAG,"record_send_data by BLE size %d.\r\n",wanted_size);
        break;
    } 
    return readSize;
}

uint8_t   preGain = 0x00, micPGA = 0x0A, ADCVolume = 0xEF, DACVolume = 0xCF;
// uint8_t   preGain = 0x03, micPGA = 0x03, ADCVolume = 0xEF, DACVolume = 0xBF;

void ParseiStethoCmd(uint8_t *cmdBuf, int cmdCnt)
{
    uint16_t *cmdTag = (uint16_t *)cmdBuf;
    ESP_LOGI("ParseCmd", "cmd[%d] = %s.", cmdCnt, cmdBuf);
    if (cmdTag[0] != 0xAA55)
    {
        ESP_LOGI("ParseCmd", "not a legitimate cmd.");
        return;
    }
    uint8_t cmd = cmdBuf[2];
    uint8_t* pData = cmdBuf+3;
    switch (cmd)
    {
        case 0x80 : {
            ESP_LOGI("ParseCmd", "A test cmd.");
            break;
        }
        case 0x39 : {
            ESP_LOGI("ParseCmd", "Current Codec PGA Gain: preGain:%02X, micGain:%02X, ADCVol:%02X, DACVol:%02X.", 
                preGain+0x30, micPGA+0x30, ADCVolume, DACVolume);
            preGain = pData[0] - 0x30;
            micPGA  = pData[1] - 0x30;
            ADCVolume = pData[2];
            DACVolume = pData[3];
            set_sysConfig();
            ESP_LOGI("ParseCmd", "New Codec PGA Gain: preGain:%02X, micGain:%02X, ADCVol:%02X, DACVol:%02X.", 
                preGain+0x30, micPGA+0x30, ADCVolume, DACVolume);
            break;
        }
        case 0x38 : {
            get_sysConfig();
            devSN[4] = 0;
            memcpy(ADVTISE_DEVICE_NAME+11, devSN, 4);
            ESP_LOGI("ParseCmd", "Get current audio PGA setting : preGain:%02X, micGain:%02X, ADCVol:%02X, DACVol:%02X.",
                preGain+0x30, micPGA+0x30, ADCVolume, DACVolume);
            // ESP_LOGI("ParseCmd", "Get current Led bright level : %d/255.", gLED_LIGHT);
            ESP_LOGI("ParseCmd", "Get current BLE device SN : %s.", ADVTISE_DEVICE_NAME);

            if (is_connected && enable_status_ntf) {
                cmdBuf[0] = 0xAA;
                cmdBuf[1] = 0x55;
                cmdBuf[2] = 0x38;
                cmdBuf[3] = preGain+0x30;
                cmdBuf[4] = micPGA+0x30;
                cmdBuf[5] = ADCVolume;
                cmdBuf[6] = DACVolume;
                cmdBuf[7] = 0;
                // cmdBuf[8] = gLED_LIGHT;
                // cmdBuf[9] = 0;
                memcpy(cmdBuf+8,  devSN, 4);
                esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_STATUS_VAL], 15, cmdBuf, false);
            }
           break;
        }
        case 0x37 : {
            pData[5] = 0;
            memcpy(ADVTISE_DEVICE_NAME+11, (char*)pData, 4); 
            memcpy((char*)(spp_adv_data+20), (char*)pData, 4); 
            memcpy(devSN, (char*)pData, 4); 
            set_sysConfig();
            ESP_LOGI("ParseCmd", "Set New Device Serial Number : %s %s.", ADVTISE_DEVICE_NAME, (char*)(spp_adv_data+9));
            esp_restart();
            break;
        }
        case 0x36 : {
            // gLED_LIGHT = pData[0];
            set_sysConfig();
            // ESP_LOGI("ParseCmd", "Set Bright level of rgbLed to %d/255.", gLED_LIGHT);
           break;
        }
        case 0x35 : {
            bat_measure(cmdBuf);
            ESP_LOGI("ParseCmd", "Get Voltage of Battery = %s.", cmdBuf);
            // memcpy(cmdBuf,  = (uint8_t *)bat_measure();
            if (is_connected && enable_status_ntf) {
                esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_STATUS_VAL], 10, cmdBuf, false);
            }
            break;
        }
        case 0x34 : {
            g_bOutputUart = pData[0];
            break;
        }
        case 0x33 : {
            esp_restart();
            break;
        }
        case 0x30 : {
            esp_deep_sleep_start();
            break;
        }
    }
}

void uart_task(void *pvParameters)
{
    uart_event_t event;

    for (;;) {
        //Waiting for UART event.
        if (xQueueReceive(spp_uart_queue, (void * )&event, (TickType_t)portMAX_DELAY)) {
            switch (event.type) {
            //Event of UART receiving data
            case UART_DATA: {
                uint8_t *temp = NULL;
                temp = (uint8_t *)malloc(sizeof(uint8_t) * event.size);
                if (temp == NULL)
                {
                    ESP_LOGE(BLE_TAG, "%s malloc.1 failed", __func__);
                    break;
                }
                memset(temp, 0x0, event.size);
                uart_read_bytes(UART_NUM_0, temp, event.size, portMAX_DELAY);

                ParseiStethoCmd(temp, event.size);

                free(temp);
            }
            break;
        default:
            break;
            }
        }
    }
    vTaskDelete(NULL);
}

static void spp_uart_init(void)
{
    uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_RTS,
        .rx_flow_ctrl_thresh = 122,
        .source_clk = UART_SCLK_DEFAULT,
    };

    //Install UART driver, and get the queue.
    uart_driver_install(UART_NUM_0, 4096, 8192, 10,&spp_uart_queue,0);
    //Set UART parameters
    uart_param_config(UART_NUM_0, &uart_config);
    //Set UART pins
    uart_set_pin(UART_NUM_0, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    xTaskCreate(uart_task, "uart_Task", 2048, (void*)UART_NUM_0, 8, NULL);
        
    ESP_LOGI(BLE_TAG, "init uart done.");
}

#ifdef SUPPORT_HEARTBEAT
void spp_heartbeat_task(void * arg)
{
    uint16_t cmd_id;

    for(;;) {
        vTaskDelay(50 / portTICK_PERIOD_MS);
        if(xQueueReceive(cmd_heartbeat_queue, &cmd_id, portMAX_DELAY)) {
            while(1){
                heartbeat_count_num++;
                vTaskDelay(5000/ portTICK_PERIOD_MS);
                if((heartbeat_count_num >3)&&(is_connected)){
                    esp_ble_gap_disconnect(spp_remote_bda);
                }
                if(is_connected && enable_heart_ntf){
                    esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_HEARTBEAT_VAL],sizeof(heartbeat_s), heartbeat_s, false);
                }else if(!is_connected){
                    break;
                }
            }
        }
    }
    vTaskDelete(NULL);
}
#endif

void ble_cmd_task(void *arg)
{
    uint8_t *cmdbuf;
    for (;;)
    {
        vTaskDelay(50 / portTICK_PERIOD_MS);
        if (xQueueReceive(ble_cmd_queue, &cmdbuf, portMAX_DELAY))
        {
            ParseiStethoCmd(cmdbuf, spp_mtu_size - 3);
            ESP_LOG_BUFFER_HEX(BLE_TAG, (char *)(cmdbuf), strlen((char *)cmdbuf));
            free(cmdbuf);
        }
    }
    vTaskDelete(NULL);
}

static void spp_task_init(void)
{
    spp_uart_init();

#ifdef SUPPORT_HEARTBEAT
    cmd_heartbeat_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(spp_heartbeat_task, "spp_heartbeat_task", 2048, NULL, 10, NULL);
#endif

    ble_cmd_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(ble_cmd_task, "ble_task", 2048, NULL, 10, NULL);
}

static void gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param)
{
    esp_err_t err;
    ESP_LOGI(BLE_TAG, "GAP_EVT, event %d", event);

    switch (event)
    {
    case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT:
        esp_ble_gap_start_advertising(&spp_adv_params);
        break;
    case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
        // advertising start complete event to indicate advertising start successfully or failed
        if ((err = param->adv_start_cmpl.status) != ESP_BT_STATUS_SUCCESS)
        {
            ESP_LOGE(BLE_TAG, "Advertising start failed: %s", esp_err_to_name(err));
        }
        break;
    default:
        break;
    }
}

static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{
    esp_ble_gatts_cb_param_t *p_data = (esp_ble_gatts_cb_param_t *)param;
    uint8_t res = 0xff;

    // ESP_LOGI(BLE_TAG, "event = %x", event);
    switch (event)
    {
    case ESP_GATTS_REG_EVT:
        ESP_LOGI(BLE_TAG, "%s %d", __func__, __LINE__);
        esp_ble_gap_set_device_name(ADVTISE_DEVICE_NAME);
        ESP_LOGI(BLE_TAG, "Advitise DevName: %s", ADVTISE_DEVICE_NAME);

        ESP_LOGI(BLE_TAG, "%s %d", __func__, __LINE__);
        esp_ble_gap_config_adv_data_raw((uint8_t *)spp_adv_data, sizeof(spp_adv_data));

        ESP_LOGI(BLE_TAG, "%s %d", __func__, __LINE__);
        esp_ble_gatts_create_attr_tab(spp_gatt_db, gatts_if, SPP_IDX_NB, SPP_SVC_INST_ID);
        break;
    case ESP_GATTS_READ_EVT:
        res = find_char_and_desr_index(p_data->read.handle);
        if (res == SPP_IDX_SPP_STATUS_VAL)
        {
            // TODO:client read the status characteristic
        }
        break;
    case ESP_GATTS_WRITE_EVT:
    {
        res = find_char_and_desr_index(p_data->write.handle);
        if (p_data->write.is_prep == false)
        {
            ESP_LOGI(BLE_TAG, "ESP_GATTS_WRITE_EVT : handle = %d.\n", res);
            if (res == SPP_IDX_SPP_COMMAND_VAL)
            {
                uint8_t *spp_cmd_buff = NULL;
                spp_cmd_buff = (uint8_t *)malloc((spp_mtu_size - 3) * sizeof(uint8_t));
                if (spp_cmd_buff == NULL)
                {
                    ESP_LOGE(BLE_TAG, "%s malloc failed", __func__);
                    break;
                }
                memset(spp_cmd_buff, 0x0, (spp_mtu_size - 3));
                memcpy(spp_cmd_buff, p_data->write.value, p_data->write.len);
                xQueueSend(ble_cmd_queue, &spp_cmd_buff, 10 / portTICK_PERIOD_MS);
            }
            else if (res == SPP_IDX_SPP_DATA_NTF_CFG)
            {
                if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x01) && (p_data->write.value[1] == 0x00))
                {
                    enable_data_ntf = true;
                    ESP_LOGI(BLE_TAG, "enable_data_ntf = 1.");
                    // if (enable_status_ntf) 
                    //     ; // setLEDColor(gLED_LIGHT-30, gLED_LIGHT-40, gLED_LIGHT-30);   //  11, white
                    // else
                    //     ; // setLEDColor(0, gLED_LIGHT-40, gLED_LIGHT-30);               //  10, cyan
                }
                else if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x00) && (p_data->write.value[1] == 0x00))
                {
                    enable_data_ntf = false;
                    ESP_LOGI(BLE_TAG, "enable_data_ntf = 0.");
                    // if (enable_status_ntf)
                    //     ; // setLEDColor(gLED_LIGHT-30, gLED_LIGHT-40, 0);               //  01, yellow
                    // else
                    //     ; // setLEDColor(0, 0, gLED_LIGHT);                             //  00, blue
                }
            }
#ifdef SUPPORT_HEARTBEAT
            else if (res == SPP_IDX_SPP_HEARTBEAT_CFG)
            {
                if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x01) && (p_data->write.value[1] == 0x00))
                {
                    enable_heart_ntf = true;
                }
                else if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x00) && (p_data->write.value[1] == 0x00))
                {
                    enable_heart_ntf = false;
                }
            }
            else if (res == SPP_IDX_SPP_HEARTBEAT_VAL)
            {
                if ((p_data->write.len == sizeof(heartbeat_s)) && (memcmp(heartbeat_s, p_data->write.value, sizeof(heartbeat_s)) == 0))
                {
                    heartbeat_count_num = 0;
                }
            }
#endif
            else if (res == SPP_IDX_SPP_DATA_RECV_VAL)
            {
#ifdef SPP_DEBUG_MODE
                ESP_LOG_BUFFER_CHAR(BLE_TAG, (char *)(p_data->write.value), p_data->write.len);
#else
                uart_write_bytes(UART_NUM_0, (char *)(p_data->write.value), p_data->write.len);
                // usb_serial_jtag_write_bytes((const void *)(p_data->write.value), p_data->write.len, portMAX_DELAY);
                ESP_LOGI(BLE_TAG, "ABF1 Write to Device: %04x.\n", (int)p_data->write.value[0]);
#endif
            }
            else if (res == SPP_IDX_SPP_STATUS_CFG)
            {
                ESP_LOGI(BLE_TAG, "ABF3 Write to Device: %04x.\n", (int)p_data->write.value[0]);
                if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x01) && (p_data->write.value[1] == 0x00))
                {
                    enable_status_ntf = true;
                    ESP_LOGI(BLE_TAG, "enable_status_ntf = 1.");
                    // if (enable_data_ntf)
                    //     ; // setLEDColor(gLED_LIGHT-30, gLED_LIGHT-40, gLED_LIGHT-30);       // 11,  white
                    // else 
                    //     ; // setLEDColor(gLED_LIGHT-30, gLED_LIGHT-40, 0);                   // 01,  yellow
                }
                else if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x00) && (p_data->write.value[1] == 0x00))
                {
                    enable_status_ntf = false;
                    ESP_LOGI(BLE_TAG, "enable_status_ntf = 0.");
                    // if (enable_data_ntf)
                    //     ; // setLEDColor(0, gLED_LIGHT-40, gLED_LIGHT-30);                   //  10, cyan
                    // else 
                    //     ; // setLEDColor(0, 0, gLED_LIGHT);                                  //  00, blue
                }
            }
        }
        else if ((p_data->write.is_prep == true) && (res == SPP_IDX_SPP_DATA_RECV_VAL))
        {
            ESP_LOGI(BLE_TAG, "ESP_GATTS_PREP_WRITE_EVT : handle = %d", res);
            store_wr_buffer(p_data);
        }
        break;
    }
    case ESP_GATTS_EXEC_WRITE_EVT:
    {
        ESP_LOGI(BLE_TAG, "ESP_GATTS_EXEC_WRITE_EVT");
        if (p_data->exec_write.exec_write_flag)
        {
            print_write_buffer();
            free_write_buffer();
        }
        break;
    }
    case ESP_GATTS_MTU_EVT:
        // spp_mtu_size = p_data->mtu.mtu;
        break;
    case ESP_GATTS_CONF_EVT:
        break;
    case ESP_GATTS_UNREG_EVT:
        break;
    case ESP_GATTS_DELETE_EVT:
        break;
    case ESP_GATTS_START_EVT:
        break;
    case ESP_GATTS_STOP_EVT:
        break;
    case ESP_GATTS_CONNECT_EVT:
        spp_conn_id = p_data->connect.conn_id;
        spp_gatts_if = gatts_if;
        is_connected = true;
        memcpy(&spp_remote_bda, &p_data->connect.remote_bda, sizeof(esp_bd_addr_t));
        // setLEDColor(0, 0, gLED_LIGHT);
#ifdef SUPPORT_HEARTBEAT
        uint16_t cmd = 0;
        xQueueSend(cmd_heartbeat_queue, &cmd, 10 / portTICK_PERIOD_MS);
#endif
        break;
    case ESP_GATTS_DISCONNECT_EVT:
        spp_mtu_size = 263;
        is_connected = false;
        enable_data_ntf = false;
        // setLEDColor(0, gLED_LIGHT, 0);
#ifdef SUPPORT_HEARTBEAT
        enable_heart_ntf = false;
        heartbeat_count_num = 0;
#endif
        esp_ble_gap_start_advertising(&spp_adv_params);
        break;
    case ESP_GATTS_OPEN_EVT:
        break;
    case ESP_GATTS_CANCEL_OPEN_EVT:
        break;
    case ESP_GATTS_CLOSE_EVT:
        break;
    case ESP_GATTS_LISTEN_EVT:
        break;
    case ESP_GATTS_CONGEST_EVT:
        break;
    case ESP_GATTS_CREAT_ATTR_TAB_EVT:
    {
        ESP_LOGI(BLE_TAG, "The number handle =%x", param->add_attr_tab.num_handle);
        if (param->add_attr_tab.status != ESP_GATT_OK)
        {
            ESP_LOGE(BLE_TAG, "Create attribute table failed, error code=0x%x", param->add_attr_tab.status);
        }
        else if (param->add_attr_tab.num_handle != SPP_IDX_NB)
        {
            ESP_LOGE(BLE_TAG, "Create attribute table abnormally, num_handle (%d) doesn't equal to HRS_IDX_NB(%d)", param->add_attr_tab.num_handle, SPP_IDX_NB);
        }
        else
        {
            memcpy(spp_handle_table, param->add_attr_tab.handles, sizeof(spp_handle_table));
            esp_ble_gatts_start_service(spp_handle_table[SPP_IDX_SVC]);
        }
        break;
    }
    default:
        break;
    }
}

static void gatts_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{
    // ESP_LOGI(BLE_TAG, "EVT %d, gatts if %d", event, gatts_if);

    /* If event is register event, store the gatts_if for each profile */
    if (event == ESP_GATTS_REG_EVT)
    {
        if (param->reg.status == ESP_GATT_OK)
        {
            spp_profile_tab[SPP_PROFILE_APP_IDX].gatts_if = gatts_if;
        }
        else
        {
            ESP_LOGI(BLE_TAG, "Reg app failed, app_id %04x, status %d", param->reg.app_id, param->reg.status);
            return;
        }
    }

    do
    {
        int idx;
        for (idx = 0; idx < SPP_PROFILE_NUM; idx++)
        {
            if (gatts_if == ESP_GATT_IF_NONE || /* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */
                gatts_if == spp_profile_tab[idx].gatts_if)
            {
                if (spp_profile_tab[idx].gatts_cb)
                {
                    spp_profile_tab[idx].gatts_cb(event, gatts_if, param);
                }
            }
        }
    } while (0);
}

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*          soft holding key                */
/* ********************************************************************************************* */

void audio_task();

#define GPIO_OUTPUT_POWERCTRL   14
#define GPIO_INPUT_PWR          4
#define GPIO_OUTPUT_OLEDRESET   18

#define GPIO_INPUT_PWRPIN_SEL (1ULL << GPIO_INPUT_PWR)
#define ESP_INTR_FLAG_DEFAULT 0
static QueueHandle_t gpio_evt_queue = NULL;

struct timeval tmKeydown, tmKeyup;
int keydown_time_ms = 0;
static void IRAM_ATTR gpioPwr_isr_handler(void *arg)
{
    uint32_t gpio_num = (uint32_t)arg;
    xQueueSendFromISR(gpio_evt_queue, &gpio_num, NULL);
}

static void gpioPwr_task(void *arg)
{
    uint32_t io_num;
    int io_vol;

    for (;;)
    {
        if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY))
        {
            io_vol = gpio_get_level(io_num);
            if (GPIO_INPUT_PWR == io_num)
            {
                if (0 == io_vol)
                { // 0 and released
                    gettimeofday(&tmKeyup, NULL);
                    keydown_time_ms = (tmKeyup.tv_sec - tmKeydown.tv_sec) * 1000 + (tmKeyup.tv_usec - tmKeydown.tv_usec) / 1000;
                    ESP_LOGI("GPIO-ISR", "Pwrkey pressed for [ %d ] ms!", keydown_time_ms);
                    if (keydown_time_ms > 3000)
                    {
                        ESP_LOGI("GPIO-ISR", "Pwrkey Release and Power down!");
                        for (int i = 0; i < 4; i++)
                        {
                            // setLEDColor(0, gLED_LIGHT, 0);
                            vTaskDelay(30);
                            // setLEDColor(0, 0, 0);
                            // vTaskDelay(30);
                        }
                        vTaskDelay(10);
                        gpio_set_level(GPIO_OUTPUT_POWERCTRL, 0); // power off
                        esp_deep_sleep_start();
                    }
                }
                else
                {
                    ESP_LOGI("GPIO-ISR", "Pwrkey Down! \n");
                    gettimeofday(&tmKeydown, NULL);
                }
            }
        }
    }
}

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void gpio_init(void)
{
    gpio_config_t cfgOut = {
        .pin_bit_mask = 1ULL << GPIO_OUTPUT_POWERCTRL | GPIO_OUTPUT_OLEDRESET,
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = 0,   // GPIO_PULLUP_ENABLE,
        .pull_down_en = 0, // GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&cfgOut);
    gpio_set_level(GPIO_OUTPUT_POWERCTRL, 1); // power on LDO
	ESP_LOGI(BLE_TAG, "Power On LDO !. ");

    // create a queue to handle gpio event from isr
    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    // start gpio task
    xTaskCreate(gpioPwr_task, "gpioPwr_task", 2048, NULL, 10, NULL);

    gpio_config_t cfgIn = {
        .pin_bit_mask = (1ULL << GPIO_INPUT_PWR),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = 0,
        .pull_down_en = 1,
        .intr_type = GPIO_INTR_ANYEDGE,
    };
    gpio_config(&cfgIn);

    gpio_set_intr_type(GPIO_INPUT_PWR, GPIO_INTR_ANYEDGE);

    gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
    // hook isr handler for specific gpio pin
    gpio_isr_handler_add(GPIO_INPUT_PWR, gpioPwr_isr_handler, (void *)GPIO_INPUT_PWR);

    adc_init();
    ESP_LOGI(BLE_TAG, "Init GPIOs Done.");
}

/* ********************************************************************************************* */

void app_main(void)
{
    esp_err_t ret;

    esp_log_level_set("*", ESP_LOG_INFO);
    // esp_log_level_set(TAG, ESP_LOG_DEBUG);

    // Initialize NVS
    ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
    {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    init_LED();

    get_sysConfig();
    memcpy(ADVTISE_DEVICE_NAME+11, devSN, 4); 
    memcpy((char*)(spp_adv_data+20), devSN, 4);
    ESP_LOGE(BLE_TAG, "SN: %s, %s.", ADVTISE_DEVICE_NAME, (char*)(spp_adv_data+9));

    esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));

    ret = esp_bt_controller_init(&bt_cfg);
    if (ret)
    {
        ESP_LOGE(BLE_TAG, "%s enable controller failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

    ret = esp_bt_controller_enable(ESP_BT_MODE_BLE);
    if (ret)
    {
        ESP_LOGE(BLE_TAG, "%s enable controller failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

    ESP_LOGI(BLE_TAG, "%s init bluetooth", __func__);

    ret = esp_bluedroid_init();
    if (ret)
    {
        ESP_LOGE(BLE_TAG, "%s init bluetooth failed: %s", __func__, esp_err_to_name(ret));
        return;
    }
    ret = esp_bluedroid_enable();
    if (ret)
    {
        ESP_LOGE(BLE_TAG, "%s enable bluetooth failed: %s", __func__, esp_err_to_name(ret));
        return;
    }

    esp_ble_gatts_register_callback(gatts_event_handler);
    esp_ble_gap_register_callback(gap_event_handler);
    esp_ble_gatts_app_register(ESP_SPP_APP_ID);

    esp_err_t local_mtu_ret = esp_ble_gatt_set_local_mtu(263);
    if (local_mtu_ret)
    {
        ESP_LOGE(BLE_TAG, "set local  MTU failed, error code = %x", local_mtu_ret);
    }

    spp_task_init();

    ESP_LOGI(BLE_TAG, "init bluetooth Done!.");

    vTaskDelay(1);
    xTaskCreate(audio_task, "audio_task", 8192, NULL, 10, NULL);

    // gpio_init();

    return;
}
